#ifndef MTDATA2_DECODER_H
#define MTDATA2_DECODER_H

#include <vector>
#include <cstdint>
#include <map>
#include <functional>

// MTDATA2数据包结构
struct MTData2Packet {
    uint8_t preamble1;      // 0xFA
    uint8_t preamble2;      // 0x36 (MID)
    uint8_t length;         // 数据长度
    std::vector<uint8_t> data;  // 数据包内容
    uint8_t checksum;       // 校验和
};

// 数据标识符定义
enum XDI_DataIdentifier {
    XDI_Temperature = 0x0810,           // 温度
    XDI_PacketCounter = 0x1020,        // 数据包计数器
    XDI_SampleTimeFine = 0x1060,        // 精细时间戳
    XDI_SampleTimeCoarse = 0x1030,     // 粗糙时间戳
    XDI_Quaternion = 0x2010,            // 四元数
    XDI_EulerAngles = 0x2030,           // 欧拉角
    XDI_Acceleration = 0x4020,          // 校准加速度
    XDI_RateOfTurn = 0x8020,           // 校准角速度
    XDI_MagneticField = 0xC020,        // 校准磁场
    XDI_FreeAcceleration = 0x4030,     // 自由加速度
    XDI_AccelerationHR = 0x4021,      // 高分辨率加速度
    XDI_RateOfTurnHR = 0x8021,        // 高分辨率角速度
    XDI_RawAccGyrMagTemp = 0x1020,    // 原始数据
    XDI_StatusWord = 0xE020,          // 状态字
    XDI_DeviceId = 0xE080,            // 设备ID
    XDI_LocationId = 0xE081           // 位置ID
};

// 解析后的IMU数据结构
struct ParsedIMUData {
    // 时间戳信息
    uint32_t sample_time_fine;      // 精细时间戳 (10kHz ticks)
    uint32_t sample_time_coarse;   // 粗糙时间戳 (seconds)
    uint32_t packet_counter;       // 数据包计数器
    
    // 姿态数据
    struct {
        float w, x, y, z;          // 四元数
        bool has_quaternion;
    } quaternion;
    
    struct {
        float roll, pitch, yaw;    // 欧拉角 (度)
        bool has_euler;
    } euler;
    
    // 传感器数据
    struct {
        float x, y, z;             // 加速度 (m/s²)
        bool has_data;
    } acceleration;
    
    struct {
        float x, y, z;             // 角速度 (rad/s)
        bool has_data;
    } rate_of_turn;
    
    struct {
        float x, y, z;             // 磁场 (归一化)
        bool has_data;
    } magnetic_field;
    
    struct {
        float x, y, z;             // 自由加速度 (m/s²)
        bool has_data;
    } free_acceleration;
    
    // 高分辨率数据
    struct {
        float x, y, z;             // 高分辨率加速度 (m/s²)
        bool has_data;
    } acceleration_hr;
    
    struct {
        float x, y, z;             // 高分辨率角速度 (rad/s)
        bool has_data;
    } rate_of_turn_hr;
    
    // 原始数据
    struct {
        uint16_t acc_x, acc_y, acc_z;     // 原始加速度
        uint16_t gyr_x, gyr_y, gyr_z;     // 原始角速度
        uint16_t mag_x, mag_y, mag_z;     // 原始磁场
        int16_t temperature;               // 原始温度 (1/256°C)
        bool has_data;
    } raw_data;
    
    // 状态信息
    uint32_t status_word;          // 状态字
    uint32_t device_id;           // 设备ID
    uint16_t location_id;         // 位置ID
    float temperature;            // 温度 (°C)
    
    // 数据有效性标志
    bool has_temperature;
    bool has_status_word;
    bool has_device_id;
    bool has_location_id;
    
    // 构造函数
    ParsedIMUData() : 
        sample_time_fine(0), sample_time_coarse(0), packet_counter(0),
        quaternion{0, 0, 0, 0, false}, euler{0, 0, 0, false},
        acceleration{0, 0, 0, false}, rate_of_turn{0, 0, 0, false},
        magnetic_field{0, 0, 0, false}, free_acceleration{0, 0, 0, false},
        acceleration_hr{0, 0, 0, false}, rate_of_turn_hr{0, 0, 0, false},
        raw_data{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, false},
        status_word(0), device_id(0), location_id(0), temperature(0),
        has_temperature(false), has_status_word(false), 
        has_device_id(false), has_location_id(false) {}
    
    // 重置所有数据
    void reset();
    
    // 格式化输出
    std::string toString() const;
};

// MTDATA2解码器类
class MTData2Decoder {
public:
    MTData2Decoder();
    ~MTData2Decoder();
    
    // 解析MTDATA2消息
    bool parseMessage(const std::vector<uint8_t>& raw_data, ParsedIMUData& imu_data);
    
    // 查找消息开始位置
    size_t findMessageStart(const std::vector<uint8_t>& data);
    
    // 验证消息完整性
    bool validateMessage(const std::vector<uint8_t>& data, size_t start_pos);
    
    // 计算校验和
    uint8_t calculateChecksum(const std::vector<uint8_t>& data, size_t start, size_t length);

private:
    // 解析数据包
    bool parseDataPacket(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    
    // 解析特定数据类型
    bool parseTemperature(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parsePacketCounter(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseSampleTimeFine(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseSampleTimeCoarse(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseQuaternion(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseEulerAngles(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseAcceleration(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseRateOfTurn(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseMagneticField(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseFreeAcceleration(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseAccelerationHR(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseRateOfTurnHR(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseRawData(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseStatusWord(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseDeviceId(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    bool parseLocationId(const std::vector<uint8_t>& data, size_t& offset, ParsedIMUData& imu_data);
    
    // 数据转换函数
    float readFloat32(const std::vector<uint8_t>& data, size_t& offset);
    uint32_t readUint32(const std::vector<uint8_t>& data, size_t& offset);
    uint16_t readUint16(const std::vector<uint8_t>& data, size_t& offset);
    int16_t readInt16(const std::vector<uint8_t>& data, size_t& offset);
    uint8_t readUint8(const std::vector<uint8_t>& data, size_t& offset);
    
    // 消息常量
    static const uint8_t PREAMBLE1 = 0xFA;
    static const uint8_t PREAMBLE2 = 0x36;
    static const size_t MIN_MESSAGE_LENGTH = 4; // 最小消息长度
    
    // 数据标识符映射
    std::map<uint16_t, std::function<bool(const std::vector<uint8_t>&, size_t&, ParsedIMUData&)>> parser_map;
};

#endif // MTDATA2_DECODER_H
